#include <rfm70.h>
#include "ROB_RFM70.h"
#include "ROB_Led.h"
#include "ROB_Telemetry.h"
#include "ROB_Pantilt.h"



void CRFM70RC::Begin(uint8_t pinRFM) {
  _rfm.Begin(pinRFM);
  _rfm.confAddrWidth(3);
  _rfm.setMode(MODE_PRX);
#ifdef RFM_DEBUG
  myDebug(0x04); // show registers
#endif
  
}




void CRFM70RC::update() {
  byte len = _rfm.receivePayload(_payload);
  if (len) {
    Led.flash(5);
#ifdef RFM_DEBUG
    myDebug(0x04);
    myDebug(0x01);
    Serial.print("payload: ");
    for (int i=0; i< len; i++){
      Serial.print(_payload[i],HEX);
      Serial.print(":");
    }
    Serial.println("");
#endif
    dispatchRCUCmd(_payload, len);
  }
}




// Dispatcher for commands from RCU
void CRFM70RC::dispatchRCUCmd(uint8_t * cmd, uint8_t len) {


  byte pos = 0;
  while(pos < len) {

    switch (cmd[pos]) {
    case ID_HEARTBEAT:

      // do nothing special here.
      // but because we received someting, we will send
      // telemetry if stream is on.
      pos+=2;
      break;
    case ID_LIGHTS:
      tmy.lightSystem = cmd[++pos];
      pos++;
      break;
    case ID_TELEMETRYENABLER:
      tmy.actorEnablers = (tmy.actorEnablers & ~(1 << TMY_ENA_DN)) | (cmd[++pos] << TMY_ENA_DN);
      pos++;
      break;
    case ID_SERVOPANTARGET:
      tmy.servoPanTargetPos = cmd[++pos];
      pos++;
      break;
    case ID_SERVOTILTTARGET:
      tmy.servoTiltTargetPos = cmd[++pos];
      pos++;
      break;
    case ID_ENGINELPOWER:
      tmy.engineLPower = (byte)cmd[++pos];
      pos++;
      break;
    case ID_ENGINERPOWER:
      tmy.engineRPower = (byte)cmd[++pos];
      pos++;
      break;
    case ID_ENGINELDIRECTION:
      tmy.engineDirections = (tmy.engineDirections & 0x0c ) | (byte)cmd[++pos];
      pos++;
      break;
    case ID_ENGINERDIRECTION:
      tmy.engineDirections = (tmy.engineDirections & 0x03 ) | (byte)(cmd[++pos] << 2);
      pos++;
      break;
    case ID_IRBARENABLER:
      tmy.actorEnablers = (tmy.actorEnablers & ~(1 << TMY_ENA_IRBAR)) | (cmd[++pos] << TMY_ENA_IRBAR);
      pos++;
      break;
    case ID_PINGENABLER:
      tmy.actorEnablers = (tmy.actorEnablers & ~(1 << TMY_ENA_PING)) | (cmd[++pos] << TMY_ENA_PING);
      pos++;
      break;
    case ID_SOUNDPLAY:
      tmy.soundPlay = cmd[++pos];
      pos++;
      break;
    case ID_CAM_ENABLER:
      tmy.actorEnablers = (tmy.actorEnablers & ~(1 << TMY_ENA_CAM)) | (cmd[++pos] << TMY_ENA_CAM);
      pos++;
      break;
    case ID_MOCS_TURN:
      tmy.mocsTurn = cmd[++pos];
      pos++;
      break;
    case ID_MOCS_DRIVESEC:
      tmy.mocsDriveSec = cmd[++pos];
      pos++;
      break;
    case ID_CAVS_ENABLER:
      tmy.actorEnablers = (tmy.actorEnablers & ~(1 << TMY_ENA_CAVS)) | (cmd[++pos] << TMY_ENA_CAVS);
      pos++;
      break;
    case ID_FAVS_ENABLER:
      tmy.actorEnablers = (tmy.actorEnablers & ~(1 << TMY_ENA_FAVS)) | (cmd[++pos] << TMY_ENA_FAVS);
      pos++;
      break;
    case ID_MOVEPANTILT:
      pos++;
      if(cmd[pos] & 0x20)
        tmy.servoPanTargetPos = SERVO_PAN_MAX_POS;
      else if (cmd[pos] & 0x10)
        tmy.servoPanTargetPos = SERVO_PAN_MIN_POS;
      else if (! (cmd[pos] & 0xF0))
        tmy.servoPanTargetPos = tmy.servoPanPos;
      if(cmd[pos] & 0x02)
        tmy.servoTiltTargetPos = SERVO_TILT_MAX_POS;
      else if (cmd[pos] & 0x01)
        tmy.servoTiltTargetPos = SERVO_TILT_MIN_POS;
      else if (! (cmd[pos] & 0xF))
        tmy.servoTiltTargetPos = tmy.servoTiltPos;
      pos++;
      break;
    case ID_BTNCZ:
      tmy.btnCZHandler = cmd[++pos];
      pos++;
      break;
    case ID_INITIALIZER:
      setup();
      pos +=2;
      ///###ODO Check MotorDriverSetup();
      break;
    case ID_STATUSREQUESTER:
      //sensorChanged = 1;
      // we will send telemetry even if no sensor changed
      pos +=2;
      break;
    }
  }


  // default response: send telemetry data downstream

  if (tmy.actorEnablers  & (1 << TMY_ENA_DN)) {
    rcuTransmit((uint8_t *)&tmy, sizeof(tmy));
  }

}




void CRFM70RC::rcuTransmit(uint8_t * cmd_buf, uint8_t len) {
  if (_rfm.txTimeout()) {
    _rfm.flushTxFIFO();
    _rfm.cliTimeout();
  }
  _rfm.sendAckPayload(cmd_buf, len);
#ifdef DEBUG
  myDebug(0x02);
  Serial.print("sending payload with len: ");
  Serial.println(len,DEC);
  Serial.print("sending payload: ");
  for (int i=0; i< len; i++){
    Serial.print(cmd_buf[i],HEX);
    Serial.print(":");
  }
  Serial.println("");
  myDebug(0x04);
#endif
}




#ifdef RFM_DEBUG
void CRFM70RC::myDebug(uint8_t code) {
  switch (code) {
  case 0x01:
    Serial.println("received payload");
    break;
  case 0x02:
    Serial.println("sent payload");
    break;
  case 0x03:
    Serial.println("timeout");
    break;
  case 0x04:
    Serial.print("debug registers: ");
    for(int j=0; j<10; j++)
    {
      Serial.print(j,HEX);
      Serial.print(":");
      Serial.print(_rfm.readRegVal(j), HEX);
      Serial.print(", ");
    }
    Serial.print(0x17,HEX);
    Serial.print(":");
    Serial.print(_rfm.readRegVal(0x17), HEX);
    Serial.print(", carrier: ");
    Serial.print(_rfm.getCarrierDetect(), HEX);
    Serial.println("");
    break;
  }

}
#endif













CRFM70RC RFM70RC;
